clear all;
close all;
xk1=[0.02;0;0.03;-0.01];xk2=[0.02;0;0.03;-0.01];xk3=[0.02;0;0.03;-0.01];xk4=[2;0;3;-1];xk5=0.0078125;
nu1=0;nu2=0;nu3=0;nu4=0;ut_1=0;ut_2=0;
T=0.001;
for k=1:1:15000
time(k)=k*T;
xd(k)=0.01;
if time(k)<=0.5
    Zeta(k)=(1-cos(pi*k*T/0.5))/2;
elseif time(k)>0.5
    Zeta(k)=1;
end
if time(k)<=0.5
    dZeta(k)=(pi*sin(pi*k*T/0.5))/(2*0.5);
elseif time(k)>0.5
    dZeta(k)=0;
end

tSpan=[0 T];
para1=nu1;      % D/A
[tt,xx]=ode45('plant',tSpan,xk1,[],para1);
xk1=xx(length(xx),:);    % A/D
x1_1(k)=xk1(1);
x2_1(k)=xk1(2);
x3_1(k)=xk1(3);
x4_1(k)=xk1(4);

para2=nu2;      % D/A
[tt,xx]=ode45('plant',tSpan,xk2,[],para2);
xk2=xx(length(xx),:);    % A/D
x1_2(k)=xk2(1);
x2_2(k)=xk2(2);
x3_2(k)=xk2(3);
x4_2(k)=xk2(4);

para3=nu3;      % D/A
[tt,xx]=ode45('plant',tSpan,xk3,[],para3);
xk3=xx(length(xx),:);    % A/D
x1_3(k)=xk3(1);
x2_3(k)=xk3(2);
x3_3(k)=xk3(3);
x4_3(k)=xk3(4);

% para4=nu4;      % D/A
% [tt,xx]=ode45('plant',tSpan,xk4,[],para4);
% xk4=xx(length(xx),:);    % A/D
% x1_4(k)=xk4(1);
% x2_4(k)=xk4(2);
% x3_4(k)=xk4(3);
% x4_4(k)=xk4(4);

para5=[ut_1,ut_2];      % D/A
[tt,xx]=ode45('z1',tSpan,xk5,[],para5);
xk5=xx(length(xx),:);    % A/D
z(k)=xk5(1);

m1=1.28;m2=1.05;k1=190;
k2=780;k3=450;c1=15;

s1(k)=m1*x2_1(k)+(x1_1(k)-xd(k))/m1;
s2(k)=m1*x2_2(k)+(x1_2(k)-xd(k))/m1;
s3(k)=m1*x2_3(k)+(x1_3(k)-xd(k))/m1;
% s4(k)=m1*x2_4(k)+3*(x1_4(k)-xd(k))/m1;

es=s3(k)-z(k);
tf=0.5;delta=0.000001;
kg=dZeta(k)/(1-Zeta(k)+delta);

Kcc=10*log(abs(s1(k))/0.002+1);
Kcv=20*atan(abs(s2(k))/0.01)/(pi/2-atan(abs(s2(k))/0.01));
Kcves=20*atan(abs(es)/0.01)/(pi/2-atan(abs(es)/0.01));
% if Kcv2<=20
%     Kbf=Kcv2;
% elseif Kcv2>20
%     Kbf=20;
% end

ut1(k)=-Kcc*sign(s1(k));
ut2(k)=-Kcv*sign(s2(k));
ut3(k)=-kg*z(k)-Kcves*sign(es);
% ut4(k)=-Kbf*sign(s3(k));

nu1=ut1(k);
nu2=ut2(k);
nu3=ut3(k);
% nu4=ut4(k);
ut_1=Zeta(k);
ut_2=dZeta(k);
end
figure(1);
plot(time,s1,'k',time,s2,'g',time,s3,'b');
legend('CONTROL(9)','CONTROL(13)','CONTROL(25)','CONTROL[21]');
xlabel('time(s)');ylabel('sliding variables');

figure(2);
plot(time,ut1,'k',time,ut2,'g',time,ut3,'b');
legend('CONTROL(9)','CONTROL(13)','CONTROL(25)','CONTROL[21]');
xlabel('time(s)');ylabel('Control input');